#!/usr/bin/python
## NAO-ROBOTICS FINAL PROJECT 
'''
	CS494 FALL 2011 
	DR. PARKER
	GROUP 3: 
		TYLER BURKLE
		TRUSHNA PATEL
		CURTIS TAYLOR
'''
#

from naoqi import *
import time
IP = '10.26.210.59' #robot's IP
PORT = 9559
speech	= ALProxy("ALTextToSpeech", IP, PORT) 
mp	= ALProxy("ALMotion", IP, PORT) 
b	= ALProxy("ALBehaviorManager", IP, PORT) 
sonar	= ALProxy("ALSonar", IP, PORT) 
mem	= ALProxy("ALMemory", IP, PORT)
red	= ALProxy("ALRedBallDetection", IP, PORT)
track	= ALProxy("ALRedBallTracker", IP, PORT)
video 	= ALProxy("ALVideoDevice", IP, PORT)

video.kCameraSelectID = 18
print video.getParam(video.kCameraSelectID)
sonar.subscribe("SonarTest", 500, 1.0) 

period = 500

#Stand up
speech.post.say("Standing Up") 
mp.setStiffnesses('Body', 1)
b.runBehavior('standUp')
mp.setWalkArmsEnable(True,True)

#Stop robot if head is tapped
headMiddle = 0
memValue = 'redBallDetected'
red.subscribe("redBallDetected", period, 0.0)
speech.post.say("Start Tracking")
track.startTracker()

video.setParam(video.kCameraSelectID, 1)

while headMiddle == 0:
	#should robot be turning while below loop is running?

	time.sleep(0.5)
	val = mem.getData(memValue, 0)	
	pos_array = track.getPosition()
	print pos_array

	x = pos_array[0]
	y = pos_array[1]
	z = pos_array[2]
	headMiddle = mem.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
	
#NEED a stop all action before trying to sit down
track.stopTracker()
#Unsubscribe ALRedBallDetection
red.unsubscribe("redBallDetected")

#Once robot is done collecting red balls, make it sit down
speech.post.say("Sitting down")
b.runBehavior("sitDown")
mp.setStiffnesses('Body', 0)

